/*
 read value from accelerometer 
 roadmap:
 - get gravitation and init axes
 - make plan with orthogonal zvector
 - detect forward and backward
 - use gyro and detect angle based on up/down
 
 
 upload:
 avrdude  -v -pm328p -carduino -b57600 -P/dev/ttyUSB0 -Uflash:w:/tmp/build8343873294074666495.tmp/ebike.cpp.hex:i
 
 see: serial.download_rate
 
 
 # read post about pedale detection
 http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1252694516/30
 
 */

/** 
 DCM
  	

As I understand your question, you know the pitch and yaw of your device 
(from the magnetometer) and want to use this information to calculate the 
component of gravity along each of your (device) coordinate axes?

As a physicist I'm brought up with Euler angles instead of pitch-yaw-roll, 
but looking at http://en.wikipedia.org/wiki/Yaw,_pitch,_and_roll I would calculate 
this as follows: Assume that your device is initially oriented along the global coordinate 
frame, so that gravity is gvec:={0,0,-g} (in the local frame). 
Now we have to calculate the local coordinates of gvec as we go through the 
yaw-pitch-roll (yaw doesn't do anything as you mention). 
To me this is easiest with rotation matrices: we have to change the sign of the angles since gvec 
stays put. I'll do this with Mathematica because that's my hammer and this is a nail

yaw = RotationMatrix[-yawangle,{0,0,1}];
pitch = RotationMatrix[-pitchangle, {0,1,0}];
roll = RotationMatrix[-rollangle,{1,0,0}];
gvec={0,0,-g}
yaw.gvec
pitch.yaw.gvec
roll.pitch.yaw.gvec

The output is the local coordinates for gvec before yaw, and after yaw, 
pitch, and roll (so last line below should be your answer):

{0,0,-g}
{0,0,-g}
{g Sin[pitchangle],0,-g Cos[pitchangle]}
{g Sin[pitchangle],-g Cos[pitchangle] Sin[rollangle],-g Cos[pitchangle] Cos[rollangle]}

 **/

#include "config.h"
#include "ebike-tools.h" 
#include "ebike-accel.h" 
#include "ebike-controller.h"
#include "NewSoftSerial/NewSoftSerial.h"


#ifdef USE_EEPROM
#include <EEPROM.h>
#endif

#ifdef CONFIG_WITH_LOGGER
  NewSoftSerial datalogger =  NewSoftSerial(P_DL_RX, P_DL_TX);
#endif

NewSoftSerial bluetooth =  NewSoftSerial(P_BT_RX, P_BT_TX);


#ifdef CONFIG_WITH_ECLIPSE
extern "C" void __cxa_pure_virtual() {  }
#endif


static float vref;
static double V[]={0.0,0.0,0.0};
static double V1[]={0.0,0.0,0.0};
static double A[]={0.0,0.0,0.0};
static double F[]={0.0,0.0,0.0};



void setup()   {                
  
  // initialize the digital pin as VCC
  pinMode(P_VCC, OUTPUT);     
  pinMode(P_GND, OUTPUT);     
  digitalWrite(P_VCC, HIGH);
  digitalWrite(P_GND, LOW);

  
  //get the vref 3.3 or 5V
  vref=read_vcc_ref()/1000;
  
  
  Serial.begin(57600);
  bluetooth.begin(19200); 
  
  accel_init();
  
  
#ifdef CONFIG_WITH_LOGGER
  datalogger.begin(9600);
  delay(1000);
  datalogger.println("time,zeta,fzeta,x,y,z,fx,ffx,ffz");  
#endif

  
}

// log data as,
// time:mm.ss, accel:XX.XX°, faccel:XX.XX°, forcex:XX.XX°, ffx:value, fz:value \n
void printXYZ(Print &serial,int sec, float zteta,float fzteta, float x,float y, float z, float fx, float ffx,float ffz){
    float time=(((int)sec/60)*100)+(sec%60);
    serial.print((time/100.00),2);  //time
    serial.print(",");        
    serial.print(zteta,2);          
    serial.print(",");
    serial.print(fzteta,2);
    serial.print("   ,");
    serial.print(x,2);
    serial.print(",");
    serial.print(y,2);
    serial.print(",");
    serial.print(z,2);

#if 0
    serial.print("   ,");
    serial.print(x1,2);
    serial.print(",");
    serial.print(y1,2);
    serial.print(",");
    serial.print(z1,2);
#endif
    
    serial.print("   ,");
    serial.print(fx,2);
    serial.print(",");
    serial.print(ffx,2);
    serial.print(",");
    serial.print(ffz,2);
    serial.println("");
}
// Graetz bridge rectifier math
/**
                // highpath filter
                function highpass(x,x1, dt, rc, y1){
                  if(x1==0.1234)return x;
                  var alpha = rc / (rc + dt);
                  return  alpha * y1 + alpha * (x - x1);
                }
                var x1=0.1234;var val=[0,0];var force=0;var out=[0,0];
                $.each(data[4].data, function(i,v){ 
                  if (i==0)force=val[1];
                  // compute custom series
                  function limit(v){return (v>0)?((v*2>5)?5:v*2):0;}
                  val=[v[0],highpass(v[1],x1,0.2,0.6,val[1])];x1=v[1];

                  force=Math.abs(val[1])*0.2+force*0.8;
                  column.data.push([val[0],(force*4)*data[7].data[i][1]]);
                  

                });
                //$.each(gseries('x').data, function(i,v){ v[1]=v[1]*10;});

*/
//




float lasttime=0, fzteta=0,ffx,ffz=0,xg=0,zteta=0;
int dt=0;
void loop()                     
{
    // no more motion
    bool reset=(fzteta>-0.8 && fzteta<0.8);
    
    
    if ((dt=millis()-lasttime)>160){
      lasttime=millis();
      if (accel_update(V,V1,F,A,vref, -1)){
        //get angle ZG based on DCM correction (direction cosine and Euler angles)
        //zteta=atan(V1[Y]/sqrt(square(V1[X])+square(V1[Z])))*r2deg;
        zteta=-A[X]*r2deg;
        fzteta=0.2*zteta+0.8*fzteta;  // low-freq ZG.angle
        
        ffx=0.2*F[X]+0.8*ffx;  // low-freq force X
        ffz=0.2*F[Z]+0.8*ffz;  // low-freq force X
        
        
        
      
        // log data: Serial, datalogger
        //printXYZ(datalogger,millis()/1000,zteta,fzteta,V1[X],V1[Y],V1[Z],F[X], ffx,ffz);
      }

    }
  

    // gyro bandwidth DC to 140Hz
    delay(7);               
}


#if 1

/**
 * http://www.arduino.cc/playground/Code/Eclipse
 */
int main(void){
	init();
	setup();
	for (;;)loop();
	return 0;
}

#endif

